/*
 * @Author: Mcfly
 * @Date: 2021-07-18 10:36:51
 * @LastEditTime: 2021-07-31 02:02:23
 * @LastEditors: Mcfly
 * @Description: NLinkTOF模块驱动
 * @FilePath: \FTTfly\BSPdriver\NLinkTOF.c
 * 本人github:https://github.com/McflyWZX
 */
#include "NLinkTOF.h"
#include "stdlib.h"
#include "string.h"
#include "Usart.h"
#include "uart.h"

#define NLinkTOF_UsartInit() ConfigureUART7()
#define NLinkTOF_UsartSend(buf, len) USART7_Send(buf, len)
#define NLinkTOF_UsartRecv(buf, len) USART7_Get(buf, len, 9999) //int32

bool NLinkTOF_UsartInited = 0x00;
NLinkTOF *NLinkTOF_Init(uint8_t id)
{
    if (!NLinkTOF_UsartInited)
    {
        NLinkTOF_UsartInit();
        NLinkTOF_UsartInited = 0x01;
    }
    NLinkTOF *tof = (NLinkTOF *)malloc(sizeof(NLinkTOF));
    memset(tof, 0x00, sizeof(NLinkTOF));
    tof->id = id;
    return tof;
}

uint8_t oringinCheckSum = 0x63;
uint8_t NLinkTOF_ReqPack[] = {0x57, 0x10, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0x63}; //第三位是ID， 最后一位是校验和
NlinkTOF_RawData NlinkTOF_DataRecv;
void NLinkTOF_Req(NLinkTOF *tof)
{
    NLinkTOF_ReqPack[4] = tof->id;
    NLinkTOF_ReqPack[7] = tof->id + 0x63;
    uint8_t recvOK = 0;
    uint8_t tryTimes = 0;
    uint16_t dataLen = &NlinkTOF_DataRecv.checkSum - &NlinkTOF_DataRecv.header + 1;
    do
    {
		while(UARTCharGetNonBlocking(UART7_BASE) != -1){};
        NLinkTOF_UsartSend(NLinkTOF_ReqPack, 8);
        if (!NLinkTOF_UsartRecv((uint8_t *)&NlinkTOF_DataRecv, dataLen))
        {
            recvOK = 0;
            continue;
        }
        else
        {
            if (NlinkTOF_DataRecv.id != tof->id)
            {
                recvOK = 0;
                continue;
            }
            else
            {
                uint8_t checkSum = 0x00;
                uint8_t *bufStart = (uint8_t *)&NlinkTOF_DataRecv;
                for (int i = 0; i < dataLen - 1; i++)
                {
                    checkSum += bufStart[i];
                }
                if (checkSum != NlinkTOF_DataRecv.checkSum)
                {
                    recvOK = 0;
                    continue;
                }
                else
                {
                    recvOK = 1;
                    int32_t temp = (int32_t)(NlinkTOF_DataRecv.distance1 | NlinkTOF_DataRecv.distance2 << 8 | NlinkTOF_DataRecv.distance3 << 16);
                    tof->length = temp / 1000.0f;
                    tof->signalStrength = NlinkTOF_DataRecv.signalStrength1;
                    tof->status = NlinkTOF_DataRecv.status;
                    continue;
                }
            }
        }
    } while (++tryTimes < 5 && !recvOK);
}
